from flask import Flask, render_template, Response
from src.camera import Camera
import numpy as np
import cv2
import glob
from src import app
from src import cam

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*8, 3), np.float32)
objp[:,:2] = np.mgrid[0:8, 0:6].T.reshape(-1,2)

def calibrate(gray):
    objpoints = np.load('objpoints.npy').astype('float32')
    imgpoints = np.load('imgpoints.npy').astype('float32')
    ret, cam.mtx, cam.dist, cam.rvecs, cam.tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
    np.save('mtx.npy', cam.mtx)
    np.save('dist.npy', cam.dists)
    np.save('rvecs.npy', cam.rvecs)
    np.save('tvecs.npy', cam.tvecs)

@app.route('/chessboard_calibration')
def chessboard_calibration():
    #tmp_zoom = cam.zoom
    #cam.zoom = 1
    #cam.set_resolution_ratio()

    objpoints = np.load('objpoints.npy').tolist()
    imgpoints = np.load('imgpoints.npy').tolist()
    # print(imgpoints)
    cam.updata()
    
    left = cam.get_img()[0]
    right = cam.get_img()[1]
    grayleft = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)
    grayright = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)

    # Find the chess board corners
    retleft, cornersleft = cv2.findChessboardCorners(grayleft, (8,6), None)
    retright, cornersright = cv2.findChessboardCorners(grayright, (8,6), None)

    # If found, add object points, image points (after refining them)
    if retleft and retright:
        print("find")

	# left
        objpoints.append(objp)

        cv2.cornerSubPix(grayleft,cornersleft,(11,11),(-1,-1),criteria)
        imgpoints.append(cornersleft)

        # Draw and display the corners
        cv2.drawChessboardCorners(left, (8,6), cornersleft, retleft)
	cam.chessboard_left = left
	# right
        objpoints.append(objp)

        cv2.cornerSubPix(grayright,cornersright,(11,11),(-1,-1),criteria)
        imgpoints.append(cornersright)

        # Draw and display the corners
        cv2.drawChessboardCorners(right, (8,6), cornersright,retright)
        cam.chessboard_right = right

        np.save('objpoints.npy', objpoints)
        np.save('imgpoints.npy', imgpoints)

        calibrate(grayleft)
        
	return '1'
    return '0'

@app.route('/chessboard_left/<int:id>.jpg')
def chessboard_left(id):

    ret, leftjpg = cv2.imencode('.jpg', cam.chessboard_left)
    return leftjpg.tobytes()

@app.route('/chessboard_right/<int:id>.jpg')
def chessboard_right(id):

    ret, rightjpg = cv2.imencode('.jpg', cam.chessboard_right)
    return rightjpg.tobytes()

@app.route('/rectified_left/<int:id>.jpg')
def rectified_left(id):

    print(objpoints.dtype)
    cam.updata()
    img = cam.get_img()[0]
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    h, w = img.shape[:2]
    newcameramtx, roi=cv2.getOptimalNewCameraMatrix(cam.mtx,cam.dist,(w,h),1,(w,h))

    # undistort
    dst = cv2.undistort(img, cam.mtx, cam.dist, None, newcameramtx)
    # crop the image
    x,y,w,h = roi
    dst = dst[y:y+h, x:x+w]
    ret, dst = cv2.imencode('.jpg', dst)
    return dst.tobytes()


